#include"MCT8316ADriver.h"
#include"MCT8316Reg.h"
#include"bsp_iic.h"

 MCT8316A_DATA mtc8316_data1;//实例两个电机结构体
 MCT8316A_DATA mtc8316_data2;
MCT8316A_DATA*get_mat8316_data_1(void)
{
  return &mtc8316_data1;
}
MCT8316A_DATA*get_mat8316_data_2(void)
{
  return &mtc8316_data2;
}
static MCT8316A_Instance_s *MCT8316A_Instance = NULL;
  MCT8316A_conf_s conf = {

      .ISD_CONFIG = {
          .ISD_SWITCH = ISD_EN,
          .BREAK_SWITCH = BREAK_EN,
          .HiZ_SWITCH = HIZ_DIS,
          .RVS_SWITCH = RVS_DR_EN,
          .RESYNC_SWITCH = RESYNC_EN,
          .STAT_BRK_SWITCH = STAT_BRK_EN,
          .STAT_DETECT_THR = STAT_DETECT_THR_25mV,
      },
      .MOTOR_STARTUP1 = {
          .MTR_STARTUP = MTR_STARTUP_Slow_First_Cycle,
          .ALIGN_RAMP_RATE = ALIGN_RAMP_RATE_5Vs,
          .ALIGN_TIME = ALIGN_TIME_6s,
          .ALIGN_CURR_THR = ALIGN_CURR_THR_0d4V,
          .IPD_CLK_FREQ = IPD_CLK_FREQ_250Hz,
          .IPD_CURR_THR = IPD_CURR_THR_0d4V,
          .IPD_RLS_MODE = IPD_RLS_MODE_Tristate,
          .IPD_ADV_ANGLE = IPD_ADV_ANGLE_60_DEG,
          .IPD_REPEAT = IPD_REPEAT_2,
          .SLOW_FIRST_CYC_FREQ = SLOW_FIRST_CYC_FREQ_0d5Hz,
      },
      .MOTOR_STARTUP2 = {
          .OL_current_limit_conf = OL_ILIMIT_CONFIG,
          .OL_ILIMIT = OL_ILIMIT_0d5V,
          .OL_DUTY_LIMIT = OL_DUTY_LIMIT_50,
          .OL_ACC_A1 = OL_ACC_A1_0d25Hzs,
          .OL_ACC_A2 = OL_ACC_A2_5Hzs2,
          .OPN_CL_HANDOFF_THR = OPN_CL_HANDOFF_THR_400Hz,//400
          .AUTO_HANDOFF = AUTO_HANDOFF_DIS,
          .FIRST_CYCLE_FREQ_SEL = FIRST_CYCLE_FREQ_SEL_SLOW,
          .MIN_DUTY = MIN_DUTY_10,
      },
      .CLOSED_LOOP1 = {
          .COMM_CONTROL = COMM_CONTROL_120DEG,
          .CL_ACC = CL_ACC_1Vs,
          .CL_DEC = CL_DEC_1Vs,
          .PWM_FREQ_OUT = PWM_FREQ_OUT_40k,
          .LD_ANGLE_POLARITY = LD_ANGLE_POLARITY_POSITIVE,
          .LD_ANGLE = (uint32_t)(10.05f / 0.12) << 1,                                       
      },
      .CLOSED_LOOP2 = {
          .FG_SEL = FG_SEL_OPENLOOP_AND_CLOSEDLOOP,
          .FG_DIV_FACTOR = FG_DIV_FACTOR_3,
          .FG_CONFIG = FG_CONFIG_THR,
          .FG_BEMF_THR = FG_BEMF_THR_1mV,
          .MTR_STOP = MTR_STOP_Recirculation,
          .MTR_STOP_BRK_TIME = MTR_STOP_BRK_TIME_1000ms,
          .ACT_SPIN_BRK_THR = ACT_SPIN_BRK_THR_7d5,
          .BRAKE_DUTY_THRESHOLD = BRAKE_DUTY_THRESHOLD_7d5,
          .AVS_SWITCH = AVS_ENABLE,
          .CBC_ILIMIT = CBC_ILIMIT_0d6V,
      },
      .CLOSED_LOOP3 = {
          .BEMF_THRESHOLD2 = BEMF_THRESHOLD2_0,
          .BEMF_THRESHOLD1 = BEMF_THRESHOLD1_200,
          .INTEG_ZC_METHOD = INTEG_ZC_METHOD_ZC_Based,
          .DEGAUSS_MAX_WIN = DEGAUSS_MAX_WIN_15,
          .DYN_DEGAUSS = DYN_DEGAUSS_EN,
          .INTEG_DUTY_THR_HIGH = INTEG_DUTY_THR_HIGH_12,
          .INTEG_DUTY_THR_LOW = INTEG_DUTY_THR_LOW_12,
          .INTEG_CYCL_THR_HIGH = INTEG_CYCL_THR_HIGH_4,
          .INTEG_CYCL_THR_LOW = INTEG_CYCL_THR_LOW_3,
          .DYN_DGS_FILT_COUNT = DYN_DGS_FILT_COUNT_3,
          .DYN_DGS_UPPER_LIM_VM = DYN_DGS_UPPER_LIM_VM_0d09V,
          .DYN_DGS_LOWER_LIM = DYN_DGS_LOWER_LIM_0d03V,
          

          
      },
      .CONST_SPEED = {
          .SPD_POWER_KP = 5500 << 20,
          .SPD_POWER_KI = 40 << 10,
          .SPD_POWER_V_MAX = SPD_POWER_V_MAX_100,
          .SPD_POWER_V_MIN = SPD_POWER_V_MIN_0,
          .CLOSED_LOOP_MODE = CLOSED_LOOP_MODE_SPEEDLOOP,
      },
      .CONST_PWR = {
          .MAX_SPEED = 55000 << 16,
          .MAX_POWER = 80 << 4,
      },
      .TRAP_CONFIG1 = {
          .OL_HANDOFF_CYCLES = OPEN_LOOP_HANDOFF_CYCLES_6,
      },
      .FAULT_CONFIG1 = {
          .MTR_LCK_MODE = MTR_LCK_MODE_9,
      },
      .FAULT_CONFIG2 = {
          .LOCK1 = LOCK1_EN,
          .LOCK2 = LOCK2_DIS,
          .LOCK3 = LOCK3_DIS,
          .LOCK_ABN_SPEED = LOCK_ABN_SPEED_4000Hz,
          .LOSS_SYNC_TIMES = LOSS_SYNC_TIMES_4,
          .NO_MTR_THR = NO_MTR_THR_0d005V,
      },
      .PIN_CONFIG1 = {
          .SPD_CTRL_MODE = SPD_CTRL_MODE_IIC,
      }
      };


   

  
    

uint8_t MCT8316A_CRC(uint8_t *data, uint8_t len)
{

}//CRC校验，暂时不启用
/* 注意LSB ，低位先行 */
void MCT8316A_SetReg(int iic_seq, MCT8316A_reg_s *reg)
{
  uint8_t tx_buff[9];
    tx_buff[0] = OP_W | CRC_DIS | DLEN_32; // Control word 0
    tx_buff[1] = 0x00; // Control word 1
    tx_buff[2] = reg->addr; // Register address
    tx_buff[3] = reg->data & 0xFF; // Data LSB
    tx_buff[4] = (reg->data >> 8) & 0xFF; // Data
    tx_buff[5] = (reg->data >> 16) & 0xFF; // Data
    tx_buff[6] = (reg->data >> 24) & 0xFF; // Data MSB
    if(iic_seq==1)
    {//判断要发给那条IIC总线
      IICTransmit_wire(tx_buff,7);
    }
    else if(iic_seq==2)
    {
      IICTransmit_wire1(tx_buff,7);
    }
    else
    {//IIC错误状态标志位记录
      iic_error_status=NO_ID;
    }
}
/*
使用iic从mct8316中读取数据，根据芯片手册规则需要先发送再读取
*/
void MCT8316A_GetReg(int iic_seq,MCT8316A_reg_s *reg)
{
  uint8_t tx_buff[9];
  memset(tx_buff, 0, 9);
  tx_buff[0] = DLEN_32; // Control word 0
  tx_buff[1] = 0x00; // Control word 1
  tx_buff[2] = ALGO_CTRL1_ADDR;
  tx_buff[3] = 0x80; // Data LSB
  if(iic_seq==1)
  {
    IICTransmit_wire(tx_buff,7);
  }
  else if(iic_seq==2)
  {
    IICTransmit_wire1(tx_buff,7);
  }
  else
  {//IIC错误状态标志位记录
      iic_error_status=NO_ID;
  }
  tx_buff[2] = reg->addr; // Register address
  tx_buff[0] = OP_R | CRC_DIS | DLEN_32; // Control word 0
  delay(100);
   if(iic_seq==1)
  {
    IICTransmit_wire(tx_buff,3);
  }
  else if(iic_seq==2)
  {
    IICTransmit_wire1(tx_buff,3);
  }
  else
  {//IIC错误状态标志位记录
      iic_error_status=NO_ID;
  }
  uint8_t rx_buff[4];
  memset(rx_buff, 0, sizeof(rx_buff));
  delay(100);
   if(iic_seq==1)
  {
    IICReceive_wire(rx_buff,4);
    reg->data = rx_buff[0] | (rx_buff[1] << 8) | (rx_buff[2] << 16) | (rx_buff[3] << 24);
  }
  else if(iic_seq==2)
  {
    IICReceive_wire1(rx_buff,4);
    // Serial.println(rx_buff[0]);
    // Serial.println(rx_buff[1]);
    // Serial.println(rx_buff[2]);
    // Serial.println(rx_buff[3]);
    reg->data = rx_buff[0] | (rx_buff[1] << 8) | (rx_buff[2] << 16) | (rx_buff[3] << 24);
  }
  else
  {//IIC错误状态标志位记录
      iic_error_status=NO_ID;
  }
}
MCT8316A_Instance_s *MCT8316A_Init(MCT8316A_conf_s *conf)
{
  //在这里将两个驱动芯片同时初始化
  MCT8316A_Instance_s *instance = (MCT8316A_Instance_s *)malloc(sizeof(MCT8316A_Instance_s));
  memset(instance, 0, sizeof(MCT8316A_Instance_s));
  instance->measure.speed = 0;
  instance->reg_message.addr = ISD_CONFIG_ADDR;
  instance->reg_message.data = conf->ISD_CONFIG.ISD_SWITCH | conf->ISD_CONFIG.BREAK_SWITCH | conf->ISD_CONFIG.HiZ_SWITCH | conf->ISD_CONFIG.RVS_SWITCH | conf->ISD_CONFIG.RESYNC_SWITCH | conf->ISD_CONFIG.STAT_BRK_SWITCH  | conf->ISD_CONFIG.STAT_DETECT_THR;
  instance->reg_message.data = 0x6F04C180;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = MOTOR_STARTUP1_ADDR;
  instance->reg_message.data = conf->MOTOR_STARTUP1.MTR_STARTUP | conf->MOTOR_STARTUP1.ALIGN_RAMP_RATE | conf->MOTOR_STARTUP1.ALIGN_CURR_THR | conf->MOTOR_STARTUP1.IPD_CLK_FREQ | conf->MOTOR_STARTUP1.IPD_CURR_THR | conf->MOTOR_STARTUP1.ALIGN_TIME | 
  conf->MOTOR_STARTUP1.IPD_RLS_MODE | conf->MOTOR_STARTUP1.IPD_ADV_ANGLE | conf->MOTOR_STARTUP1.IPD_REPEAT | conf->MOTOR_STARTUP1.SLOW_FIRST_CYC_FREQ;
  // instance->reg_message.data = 0x35489195;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = MOTOR_STARTUP2_ADDR;
  // instance->reg_message.data = 0x1890B990 & 0x00001FC0;
  instance->reg_message.data = conf->MOTOR_STARTUP2.OL_current_limit_conf | conf->MOTOR_STARTUP2.OL_ILIMIT | conf->MOTOR_STARTUP2.OL_DUTY_LIMIT | conf->MOTOR_STARTUP2.OL_ACC_A1 | conf->MOTOR_STARTUP2.OL_ACC_A2 | conf->MOTOR_STARTUP2.MIN_DUTY | 
  conf->MOTOR_STARTUP2.OPN_CL_HANDOFF_THR | conf->MOTOR_STARTUP2.AUTO_HANDOFF | conf->MOTOR_STARTUP2.FIRST_CYCLE_FREQ_SEL;
  // instance->reg_message.data = 0x1A295990;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = CLOSED_LOOP1_ADDR;
  instance->reg_message.data = conf->CLOSED_LOOP1.COMM_CONTROL | conf->CLOSED_LOOP1.CL_ACC | conf->CLOSED_LOOP1.CL_DEC | conf->CLOSED_LOOP1.PWM_FREQ_OUT | conf->CLOSED_LOOP1.LD_ANGLE_POLARITY | conf->CLOSED_LOOP1.LD_ANGLE;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = CLOSED_LOOP2_ADDR;
  instance->reg_message.data = conf->CLOSED_LOOP2.FG_SEL | conf->CLOSED_LOOP2.FG_DIV_FACTOR | conf->CLOSED_LOOP2.CBC_ILIMIT | conf->CLOSED_LOOP2.FG_CONFIG | conf->CLOSED_LOOP2.FG_BEMF_THR | conf->CLOSED_LOOP2.MTR_STOP | 
  conf->CLOSED_LOOP2.MTR_STOP_BRK_TIME | conf->CLOSED_LOOP2.AVS_SWITCH | conf->CLOSED_LOOP2.ACT_SPIN_BRK_THR | conf->CLOSED_LOOP2.BRAKE_DUTY_THRESHOLD;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = CLOSED_LOOP3_ADDR;
  instance->reg_message.data = conf->CLOSED_LOOP3.BEMF_THRESHOLD2 | conf->CLOSED_LOOP3.BEMF_THRESHOLD1 | conf->CLOSED_LOOP3.DYN_DEGAUSS | conf->CLOSED_LOOP3.DEGAUSS_MAX_WIN | conf->CLOSED_LOOP3.INTEG_ZC_METHOD | 
  conf->CLOSED_LOOP3.DEGAUSS_MAX_WIN | conf->CLOSED_LOOP3.DYN_DGS_FILT_COUNT | conf->CLOSED_LOOP3.DYN_DGS_LOWER_LIM | conf->CLOSED_LOOP3.DYN_DGS_UPPER_LIM_VM | conf->CLOSED_LOOP3.INTEG_CYCL_THR_HIGH | 
  conf->CLOSED_LOOP3.INTEG_DUTY_THR_HIGH | conf->CLOSED_LOOP3.INTEG_CYCL_THR_LOW | conf->CLOSED_LOOP3.INTEG_DUTY_THR_LOW;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = 0x0000008C;
  instance->reg_message.data = 0x000CE944;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = CONST_SPEED_ADDR;
  instance->reg_message.data = conf->CONST_SPEED.CLOSED_LOOP_MODE | conf->CONST_SPEED.SPD_POWER_KP | conf->CONST_SPEED.SPD_POWER_KI | conf->CONST_SPEED.SPD_POWER_V_MAX | conf->CONST_SPEED.SPD_POWER_V_MIN;
  // instance->reg_message.data = 0x00A00504;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = CONST_PWR_ADDR;
  instance->reg_message.data = conf->CONST_PWR.MAX_POWER | conf->CONST_PWR.MAX_SPEED;
  // instance->reg_message.data = 0x5DC04C84;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = 0x00000096;
  instance->reg_message.data = 0x36DB6DA6;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = 0x00000098;
  instance->reg_message.data = 0x36DB6D80;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = TRAP_CONFIG1_ADDR;
  instance->reg_message.data = 0x054BA106 & 0xFF3FFFFF | conf->TRAP_CONFIG1.OL_HANDOFF_CYCLES;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = 0x0000009C;
  instance->reg_message.data = 0x62880000;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = FAULT_CONFIG1_ADDR;
  instance->reg_message.data = 0x78F43025 & 0xffffff87 | conf->FAULT_CONFIG1.MTR_LCK_MODE;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = FAULT_CONFIG2_ADDR;
  instance->reg_message.data = conf->FAULT_CONFIG2.LOCK1 | conf->FAULT_CONFIG2.LOCK2 | conf->FAULT_CONFIG2.LOCK3 | conf->FAULT_CONFIG2.LOCK_ABN_SPEED | conf->FAULT_CONFIG2.LOSS_SYNC_TIMES | conf->FAULT_CONFIG2.NO_MTR_THR;
  instance->reg_message.data = 0x0003A009;
  instance->reg_message.data = instance->reg_message.data | conf->FAULT_CONFIG2.LOCK1 | conf->FAULT_CONFIG2.LOCK2 | conf->FAULT_CONFIG2.LOCK3 | conf->FAULT_CONFIG2.LOCK_ABN_SPEED | conf->FAULT_CONFIG2.LOSS_SYNC_TIMES | conf->FAULT_CONFIG2.NO_MTR_THR;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = GD_CONFIG1_ADDR;
  instance->reg_message.data = 0x1C440000;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = GD_CONFIG2_ADDR;
  instance->reg_message.data = 0x14000000;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = PIN_CONFIG1_ADDR;
  instance->reg_message.data = conf->PIN_CONFIG1.SPD_CTRL_MODE | conf->PIN_CONFIG1.DACOUT1_VAR_ADDR | conf->PIN_CONFIG1.DACOUT2_VAR_ADDR;
  instance->reg_message.data = 0x2D720604;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->reg_message.addr = PIN_CONFIG2_ADDR;
  instance->reg_message.data = 0x080C0000;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  instance->CRC_buff = 0;
  instance->reg_message.addr = ALGO_CTRL1_ADDR;
  instance->reg_message.data = 0x80000000;
  MCT8316A_SetReg(IIC_1, &instance->reg_message);
  MCT8316A_SetReg(IIC_2, &instance->reg_message);
  delay(10);
  MCT8316A_Instance = instance;
  return instance;
}

void MCT8316_SPEED_SET_1(MCT8316A_DATA *data,uint32_t set_speed)
{
  //先做一个速度输入数值保护
  data->CMD_SPEED=set_speed;
  if(data->CMD_SPEED>=90)
  {
    data->CMD_SPEED=90;
  }
  else if(data->CMD_SPEED<=0)
  {
    data->CMD_SPEED=0;
  }
  MCT8316A_reg_s Reg;//要发送给芯片的数据
  MCT8316A_reg_s SYS_Reg;//实例化存储空间接收IIC传来数据
  SYS_Reg.addr = SYS_STATUS2_ADDR;
  MCT8316A_GetReg(IIC_1, &SYS_Reg);
  data->State=static_cast<MCT8316A_State_e>((SYS_Reg.data & 0xF0000000) >> 28);
  Reg.addr = DEVICE_CTRL_ADDR;
  Reg.data = (data->CMD_SPEED * 32767 / 100) << 16;
  MCT8316A_SetReg(IIC_1, &Reg);
}
void MCT8316_SPEED_SET_2(MCT8316A_DATA *data,uint32_t set_speed)
{
  data->CMD_SPEED=set_speed;
  if(data->CMD_SPEED>=90)
  {
    data->CMD_SPEED=90;
  }
  else if(data->CMD_SPEED<=0)
  {
    data->CMD_SPEED=0;
  }
  MCT8316A_reg_s Reg;
  MCT8316A_reg_s SYS_Reg;//实例化存储空间接收IIC传来数据
  SYS_Reg.addr = SYS_STATUS2_ADDR;
  MCT8316A_GetReg(IIC_2, &SYS_Reg);
  data->State=static_cast<MCT8316A_State_e>((SYS_Reg.data & 0xF0000000) >> 28);
  Reg.addr = DEVICE_CTRL_ADDR;
  Reg.data = (data->CMD_SPEED * 32767 / 100) << 16;
  MCT8316A_SetReg(IIC_2, &Reg);
}